#include <stdio.h>
#include <math.h>

#include "../geom/Point3D.h"
#include "../geom/Vector3D.h"
#include "../geom/Segment3D.h"
#include "../geom/Triangle.h"
#include "../geom/BBox.h"
#include "../geom/Octree.h"
#include "../UnitTest++/src/UnitTest++.h"

using namespace std;
using namespace geom;

SUITE(PointTests)
{
    TEST(CreatePoints)
    {
        //! \test Create points using its constructors
        const Point3D p1(1,1,1);
        const float ap1[3]={1,1,1};
        CHECK_ARRAY_EQUAL(p1.v,ap1,3);

        const float pv[3] = {1,1,1};
        const Point3D p2(pv);
        CHECK_ARRAY_EQUAL(p2.v,pv,3);
        CHECK_ARRAY_EQUAL(p1.v,p2.v,3);
    }

    TEST(PointsDistance)
    {
        const float threshold = 1e-07;
        const Point3D p1(1,1,1);
        const Point3D p2(2,2,2);
        CHECK_CLOSE(p1.distance(p2), sqrt(3),threshold);
        CHECK_CLOSE(p1.distance(p2), sqrt(p1.distanceSquared(p2)), threshold);
        CHECK_CLOSE(p1.distance(p2), p2.distance(p1), threshold);
    }

    TEST(PointsEqual)
    {
        const Point3D p1(10,10,10);
        const Point3D p2(10,10,10);
        const Point3D p3(10,10,10+1e-06);
        const Point3D p4(10,10,10+9e-08);
        
        CHECK(p1 == p2);
        CHECK(!(p1==p3));
        CHECK(p1!=p3);
        CHECK(p1==p4);
    }
}

SUITE(VectorTests)
{
    TEST(CreateVectors)
    {
        const float threshold = 1e-06;
        const Vector3D v1(1,1,1);
        CHECK_CLOSE(v1.module(),sqrt(3),threshold);
        
        const Vector3D v2(-1,-1,-1);
        CHECK_CLOSE(v1.module(),v2.module(),threshold);
        
        const Vector3D v3(0,0,0);
        CHECK_CLOSE(v3.module(),0,threshold);

        Vector3D v4 = v1*5;
        CHECK_CLOSE(v4.module(),v1.module()*5,threshold);

        Vector3D v5 = v1/3;
        CHECK_CLOSE(v5.module(),v1.module()/3.0,threshold);

        Vector3D v6 = v1+v2;
        const float azero[3]={0,0,0};
        CHECK_ARRAY_CLOSE(v6.dv, azero, 3, threshold);
    }

	TEST(VectorModule)
	{
        const float threshold = 1e-06;
		Vector3D v(10,10,0);
		Vector3D v2 = v.getWithModule(sqrt(2));
		const float aone[3]={1,1,0};
		CHECK_ARRAY_CLOSE(v2.dv, aone, 3, threshold);

		const Vector3D v3(1,1,0);
		Vector3D v4 = v3.getWithModule(5000);

		CHECK_CLOSE(v4.module(), 5000, threshold);
	}

    TEST(VectorProduct)
    {
        const float threshold = 1e-06;
        const Vector3D v1(0,1,0);
        const Vector3D v2(-1,0,0);
        Vector3D v3 = v1.vectorProduct(v2);
        const float av3[3] = {0,0,1};
        CHECK_ARRAY_CLOSE(v3.dv,av3,3,threshold);

        Vector3D nv3 = v2.vectorProduct(v1);
        const float anv3[3] = {0,0,-1};
        CHECK_ARRAY_CLOSE(nv3.dv,anv3,3,threshold);

        const Vector3D v4(0,0,0);
        Vector3D v5 = v1.vectorProduct(v4);
        const float azero[3]={0,0,0};
        CHECK_ARRAY_CLOSE(v5.dv,azero,3,threshold);
 
        Vector3D v6 = v4.getWithModule(100);
        CHECK_ARRAY_CLOSE(v6.dv,azero,3,threshold);

        Vector3D v7 = v1.getWithModule(100);
        float av7[3] = {0,100,0};
        CHECK_ARRAY_CLOSE(v7.dv,av7,3,threshold);
	}

    TEST(DotProduct)
    {
        const float threshold = 1e-06;
        const Vector3D v1(0,1,0);
        const Vector3D v2(1,0,0);
        const Vector3D v3(0,0,1);
        CHECK_CLOSE(v1.dotProduct(v2),0,threshold);
        CHECK_CLOSE(v1.dotProduct(v3),0,threshold);
        CHECK_CLOSE(v2.dotProduct(v3),0,threshold);
    
        const Vector3D v4(1,1,1);
        CHECK_CLOSE(v1.dotProduct(v4),1,threshold);
        CHECK_CLOSE(v4.dotProduct(v4),3,threshold);
    }

    TEST(PointPlusVector)
    {
        const float threshold = 1e-06;
        const Vector3D v(1,1,1);
        const Point3D p1(0,1,0);
        Point3D p2 = p1+v;
        CHECK_CLOSE(p2.x,1,threshold);
        CHECK_CLOSE(p2.y,2,threshold);
        CHECK_CLOSE(p2.z,1,threshold);
    }
}

SUITE(SegmentsTests)
{
    TEST(CreateSegments)
    {
        const float threshold = 1e-06;
        const Point3D p1(0,0,0);
        const Point3D p2(1,1,1);
        const Segment3D s1(p1,p2);
        CHECK_CLOSE(s1.start().x,0,threshold);
        CHECK_CLOSE(s1.start().y,0,threshold);
        CHECK_CLOSE(s1.start().z,0,threshold);

        CHECK_CLOSE(s1.end().x,1,threshold);
        CHECK_CLOSE(s1.end().y,1,threshold);
        CHECK_CLOSE(s1.end().z,1,threshold);

        Vector3D d = s1.direction();
        float mod = s1.end().distance(s1.start());
        d = d.getWithModule(mod);

        const Segment3D s2(p1,d);
        CHECK_CLOSE(s2.start().x,0,threshold);
        CHECK_CLOSE(s2.start().y,0,threshold);
        CHECK_CLOSE(s2.start().z,0,threshold);

        CHECK_CLOSE(s2.end().x,1,threshold);
        CHECK_CLOSE(s2.end().y,1,threshold);
        CHECK_CLOSE(s2.end().z,1,threshold);
    }
}

SUITE(TrianglesTests)
{
    TEST(TriangleNormal)
    {
        const float threshold = 1e-06;
        const Point3D p1(-1,0,0),
                      p2(0,1,0),
                      p3(1,0,0);
        const Triangle t(p1,p2,p3);
        Vector3D v = t.getNormal();
        const float av[3] = {0,0,1};
        CHECK_ARRAY_CLOSE(v.dv,av,3,threshold);
    }
    TEST(TriangleAdyaency)
    {
        const Point3D p1(-1,0,0),
                      p2(0,1,0),
                      p3(1,0,0);

        const Point3D p4(3,0,0),
                      p5(0,2,0),
                      p6(4,1,0);

        const Triangle t1(p1,p2,p3);
        const Triangle t2(p1,p4,p5);
        const Triangle t3(p4,p1,p5);
        const Triangle t4(p4,p5,p1);
        const Triangle t5(p4,p5,p6);
        
        CHECK(t1.isAdyacentTo(t2)); 
        CHECK(t1.isAdyacentTo(t3)); 
        CHECK(t1.isAdyacentTo(t4)); 
        CHECK(!t1.isAdyacentTo(t5)); 
    }
	TEST(TriangleRayIntersection)
	{
        const float threshold = 1e-07;
		
		Point3D p1(-1,0,0),
				p2(0,1,0),
				p3(1,0,0);
		Triangle t1(p1,p2,p3);

		Point3D p4(0,0.5,-10);
		Vector3D v(0,0,1);
		Ray r(p4,v);
		CHECK(t1.isIntersectedBy(r));

		Point3D p14(0,0.5,10);
		Vector3D v12(0,0,-1);
		Ray r12(p14,v12);
		CHECK(t1.isIntersectedBy(r12));

	

		Point3D res;
		t1.getRayIntersectionPoint(r,&res );
		const float point[3] ={0,0.5,0};
		CHECK_ARRAY_CLOSE(res.v,point,3,threshold);


		Point3D p5(0,1,-10);
		Ray r2(p5,v);
		CHECK(t1.isIntersectedBy(r2));
		int returnV = t1.getRayIntersectionPoint(r2,&res);
		const float point2[3] = {0,1,0};
		CHECK_ARRAY_CLOSE(res.v, point2,3, threshold);
		CHECK(returnV == 1);


		Point3D p6(0,1.0001,-10);
		Ray r3(p6,v);
		CHECK(!t1.isIntersectedBy(r3));
		
		Point3D p7(0,0,100);
		Vector3D v2(0,0,-1);
		Ray r4(p7,v2);
		CHECK(t1.isIntersectedBy(r4));
	}
}


SUITE(BBoxTests)
{
    TEST(BoxContainsPoints)
    {
        const Point3D center(0,0,0);

        const BBox b(center, 10);

        const Point3D p1(0,2,3);
        CHECK(b.contains(p1));

        const Point3D p2(5,0,0);
        CHECK(b.contains(p2));

        const Point3D p3(5,5,5);
        CHECK(b.contains(p3));

        const Point3D p4(5,5,5.00001);
        CHECK(!b.contains(p4));
    }

    TEST(BoxContainsTriangles)
    {
        const Point3D c(0,0,0);
        const BBox b(c,10);

        const Point3D p1(0,5,0);
        const Point3D p2(5,5,0);
        const Point3D p3(0,-5,-5);

        const Triangle t1(p1,p2,p3);
        CHECK(b.contains(t1));

        const Point3D p4(5.01,5,5);
        const Point3D p5(-5,-5,-5.0001);
        const Point3D p6(0,0,10);

        const Triangle t2(p1,p2,p4);
        const Triangle t3(p1,p4,p5);
        const Triangle t4(p4,p5,p6);
        CHECK(!b.contains(p4));
        CHECK(!b.contains(p5));
        CHECK(!b.contains(p6));
    }
	TEST(RayIntersectsBox)
	{
		const Point3D p1(10,5.0001,5);
		const Vector3D v(-1,0,0);
		const Ray r1(p1,v);

		const Point3D c(0,0,0);
		const BBox b(c, 10);

		CHECK(!b.isTraversedBy(r1));

		const Point3D p2(10,4,5);
		const Ray r2(p2,v);
		CHECK(b.isTraversedBy(r2));

		const Point3D p3(10,5,5);
		const Ray r3(p3,v);
		CHECK(b.isTraversedBy(r3));


		const Point3D p4( 0,0,10);
		const Vector3D v4(-1,0,-1);
		const Ray r4 (p4,v4);
		CHECK(b.isTraversedBy(r4));

		const Point3D p5( 0,0,10);
		const Vector3D v5(-1,0,-0.99999);
		const Ray r5 (p5,v5);
		CHECK(!b.isTraversedBy(r5));
	}
}

SUITE(RayTests)
{
	TEST(Rays)
	{
        const float threshold = 1e-07;
		const Point3D o(0,0,0);
		const Vector3D v(0,1,0);
		const Ray r(o,v);

		const float res[3]={0,5,0};
		Point3D mp = r.atDistance(5);
		CHECK_ARRAY_CLOSE (mp.v, res, 3, threshold);
	}

}



SUITE(OctreeTests)
{
    TEST(OctreeContainsPoints)
    {
        const Point3D c(0,0,0);
        const BBox box(c,10);

        const Octree<Point3D> oct(box,3);

        const Point3D p1(5,5,5);
        CHECK(oct.contains(p1));

        const Point3D p2(5,5,-5.01);
        CHECK(!oct.contains(p2));
    }

	TEST(OctreeFindIntersections)
	{
        const float threshold = 1e-07;
		const Point3D c(0,0,0);
		const BBox box(c,10);

		Octree<Triangle> oct(box,3);

		Point3D p1(-3,0,0),
				p2(-2,1,0),
				p3(-1,0,0);
		Triangle t1(p1,p2,p3);

		Point3D p4(-3,0,1),
				p5(-2,1,1),
				p6(-1,0,1);
		Triangle t2(p4,p5,p6);

		Point3D p7(-3,0,2),
				p8(-2,1,2),
				p9(-1,0,2);
		Triangle t3(p7,p8,p9);

		oct.addObject(t1);
		oct.addObject(t2);
		oct.addObject(t3);
	
		Point3D p10(-2,0.5,-10);
		Vector3D v1(0,0,1);
		Ray r(p10,v1);
		Point3D tempP;
		bool found = oct.findNearestIntersection(r, &tempP);
		CHECK(found);
		float intPoint[3]={-2,0.5,0};
		CHECK_ARRAY_CLOSE(tempP.v,intPoint,3,threshold);


		Point3D p11(-2.5,0.5,40);
		Vector3D v2(0,0,-1);
		Ray r2(p11,v2);
		
		Point3D tempP2;
		found = oct.findNearestIntersection(r2, &tempP2);
		CHECK(found);
		float intPoint2[3]={-2.5,0.5,2};
		CHECK_ARRAY_CLOSE(tempP2.v,intPoint2,3,threshold);

	}

}
